﻿import smbus2,time

class mpu6050_i2c:
    
    def __init__(self):
        # MPU6050 Registers
        self.MPU6050_ADDR = 0x68
        self.PWR_MGMT_1   = 0x6B
        self.SMPLRT_DIV   = 0x19
        self.CONFIG       = 0x1A
        self.GYRO_CONFIG  = 0x1B
        self.ACCEL_CONFIG = 0x1C
        self.INT_ENABLE   = 0x38
        self.ACCEL_XOUT_H = 0x3B
        self.ACCEL_YOUT_H = 0x3D
        self.ACCEL_ZOUT_H = 0x3F
        self.TEMP_OUT_H   = 0x41
        self.GYRO_XOUT_H  = 0x43
        self.GYRO_YOUT_H  = 0x45
        self.GYRO_ZOUT_H  = 0x47
       
       
        #姿态角计算暂时不接入--有漂移误差

        # 启动总线连接
        self.gyro_sens,self.accel_sens = self.connect_i2c()
    
     
     
    def connect_i2c(self):
        self.bus = smbus2.SMBus(0) # 其实就是指定驱动的编号文件 貌似还有个1
        #alter sample rate
        samp_rate_div = 0
        self.bus.write_byte_data(self.MPU6050_ADDR, self.SMPLRT_DIV, samp_rate_div)
        time.sleep(0.1)
        # reset all sensors
        self.bus.write_byte_data(self.MPU6050_ADDR,self.PWR_MGMT_1,0x00)
        time.sleep(0.1)
        # power management and crystal settings
        self.bus.write_byte_data(self.MPU6050_ADDR, self.PWR_MGMT_1, 0x01)
        time.sleep(0.1)
        #Write to Configuration register
        self.bus.write_byte_data(self.MPU6050_ADDR, self.CONFIG, 0)
        time.sleep(0.1)
        #Write to Gyro configuration register
        gyro_config_sel = [0b00000,0b010000,0b10000,0b11000] # byte registers
        gyro_config_vals = [250.0,500.0,1000.0,2000.0] # degrees/sec
        gyro_indx = 0
        self.bus.write_byte_data(self.MPU6050_ADDR, self.GYRO_CONFIG, int(gyro_config_sel[gyro_indx]))
        time.sleep(0.1)
        #Write to Accel configuration register
        accel_config_sel = [0b00000,0b01000,0b10000,0b11000] # byte registers
        accel_config_vals = [2.0,4.0,8.0,16.0] # g (g = 9.81 m/s^2)
        accel_indx = 0
        self.bus.write_byte_data( self.MPU6050_ADDR,  self.ACCEL_CONFIG, int(accel_config_sel[accel_indx]))
        time.sleep(0.1)
        # interrupt register (related to overflow of data [FIFO])
        self.bus.write_byte_data(self.MPU6050_ADDR, self.INT_ENABLE, 1)
        time.sleep(0.1)
        return gyro_config_vals[gyro_indx],accel_config_vals[accel_indx]
        
    
    # 读取数据
    def read_raw_bits(self,register):
        # read accel and gyro values
        high = self.bus.read_byte_data(self.MPU6050_ADDR, register)
        low = self.bus.read_byte_data(self.MPU6050_ADDR, register+1)
    
        # combine higha and low for unsigned bit value
        value = ((high << 8) | low)
    
        # convert to +- value
        if(value > 32768):
            value -= 65536
        return value


    def read_mpu_data(self):
        # raw acceleration bits
        acc_x = self.read_raw_bits(self.ACCEL_XOUT_H)
        acc_y = self.read_raw_bits(self.ACCEL_YOUT_H)
        acc_z = self.read_raw_bits(self.ACCEL_ZOUT_H)

        # raw gyroscope bits
        gyro_x = self.read_raw_bits(self.GYRO_XOUT_H)
        gyro_y = self.read_raw_bits(self.GYRO_YOUT_H)
        gyro_z = self.read_raw_bits(self.GYRO_ZOUT_H)

        # raw temp bits
        ##    t_val = read_raw_bits(TEMP_OUT_H) # uncomment to read temp

        #convert to acceleration in g and gyro dps
        a_x = (acc_x/(2.0**15.0))*self.accel_sens
        a_y = (acc_y/(2.0**15.0))*self.accel_sens
        a_z = (acc_z/(2.0**15.0))*self.accel_sens
    
        w_x = (gyro_x/(2.0**15.0))*self.gyro_sens
        w_y = (gyro_y/(2.0**15.0))*self.gyro_sens
        w_z = (gyro_z/(2.0**15.0))*self.gyro_sens

        ##    temp = ((t_val)/333.87)+21.0 

        return a_x,a_y,a_z,w_x,w_y,w_z
    
    def read_gyroscope(self,dt):
        gyro_z = self.read_raw_bits(self.GYRO_ZOUT_H)
        return (gyro_z / 131.0) * dt# 将原始数据转换为角速度 (°/s)
    
    #静止状态的偏置量
    def calc_GyroOffsets(self):
        x=0
        y=0
        z=0
        for i in range(2000):
            a_x,a_y,a_z,w_x,w_y,w_z = self.read_mpu_data()
            x += w_x
            y += w_y
            z += w_z
        
        gx_off = x/2000
        gy_off = y/2000
        gz_off = z/2000
        
        return gx_off,gy_off,gz_off
    
    def calc_angleAcc(self):
        acc_x,acc_y,acc_z,w_x,w_y,w_z = self.read_mpu_data()
        